#include "GraphSearch.h"

using namespace common::planning;

//初始化搜索空间
void GraphSearch::InitSearch() {
  open_list_f_.clear();
  open_list_f_.insert(start_);
  expanded_.assign(map_.Width() * map_.Height(), GridXY());
  expanded_.at(start_->x + start_->y * map_.Width()).status = GridXY::OPEN;
  expanded_.at(start_->x + start_->y * map_.Width()).np = start_;
  closed_list_size_ = 0;
  path_.clear();
}

//弹出open_list中最优(估价值f最小)的节点并放入closed_list
void GraphSearch::PopOpen(Node::Ptr& np) {
  np = *(open_list_f_.begin());
  open_list_f_.erase(open_list_f_.begin());
  expanded_.at(np->x + np->y * map_.Width()).status = GridXY::CLOSED;
  expanded_.at(np->x + np->y * map_.Width()).np = np;
  closed_list_size_ ++;
}

Nodes GraphSearch::PruneNeighbours(const Node::Ptr& np) const {
  Nodes neighbours{};
  switch (np->dir) {
    case Node::NONE: {
      neighbours = PruneNeighboursNone(np);
      break;
    }
    case Node::RIGHT: {
      neighbours = PruneNeighboursRight(np);
      break;
    }
    case Node::RIGHT_UP: {
      neighbours = PruneNeighboursRightUp(np);
      break;
    }
    case Node::UP: {
      neighbours = PruneNeighboursUp(np);
      break;
    }
    case Node::LEFT_UP: {
      neighbours = PruneNeighboursLeftUp(np);
      break;
    }
    case Node::LEFT: {
      neighbours = PruneNeighboursLeft(np);
      break;
    }
    case Node::LEFT_DOWN: {
      neighbours = PruneNeighboursLeftDown(np);
      break;
    }
    case Node::DOWN: {
      neighbours = PruneNeighboursDown(np);
      break;
    }
    case Node::RIGHT_DOWN: {
      neighbours = PruneNeighboursRightDown(np);
      break;
    }
    default: {
      break;
    }
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursNone(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursRight(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1) && 
      map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1) &&
      map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursRightUp(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1) &&
      map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1) &&
      map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursUp(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1) &&
      map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1) &&
      map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursLeftUp(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1) &&
      map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1) &&
      map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursLeft(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1) &&
      map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1) &&
      map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursLeftDown(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y + 1) &&
      map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::LEFT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1) &&
      map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursDown(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1) &&
      map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1) &&
      map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Nodes GraphSearch::PruneNeighboursRightDown(const Node::Ptr& np) const {
  Nodes neighbours{};
  if(!map_.Occupied(np->x, np->y - 1)) {
    Node node(*np);
    node.dir = Node::DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y - 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_DOWN;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y)) {
    Node node(*np);
    node.dir = Node::RIGHT;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x + 1, np->y + 1) &&
      map_.Occupied(np->x, np->y + 1)) {
    Node node(*np);
    node.dir = Node::RIGHT_UP;
    neighbours.push_back(node);
  }
  if(!map_.Occupied(np->x - 1, np->y - 1) &&
      map_.Occupied(np->x - 1, np->y)) {
    Node node(*np);
    node.dir = Node::LEFT_DOWN;
    neighbours.push_back(node);
  }

  return neighbours;
}

Node GraphSearch::Step(const Node& node) const {
  Node next_node(node);
  switch (node.dir) {
    case Node::RIGHT: {
      next_node.x += 1;
      next_node.g += 1.0;
      break;
    }
    case Node::RIGHT_UP: {
      next_node.x += 1;
      next_node.y += 1;
      next_node.g += M_SQRT2;
      break;
    }
    case Node::UP: {
      next_node.y += 1;
      next_node.g += 1.0;
      break;
    }
    case Node::LEFT_UP: {
      next_node.x -= 1;
      next_node.y += 1;
      next_node.g += M_SQRT2;
      break;
    }
    case Node::LEFT: {
      next_node.x -= 1;
      next_node.g += 1.0;
      break;
    }
    case Node::LEFT_DOWN: {
      next_node.x -= 1;
      next_node.y -= 1;
      next_node.g += M_SQRT2;
      break;
    }
    case Node::DOWN: {
      next_node.y -= 1;
      next_node.g += 1.0;
      break;
    }
    case Node::RIGHT_DOWN: {
      next_node.x += 1;
      next_node.y -= 1;
      next_node.g += M_SQRT2;
      break;
    }
    case Node::NONE:
    default: {
      printf("[GraphSearch.cpp] error dir %d\n", node.dir);
      break;
    }
  }
  return next_node;
}

bool GraphSearch::Forced(const Node::Ptr np) const {
  bool forced = false;
  switch (np->dir) {
    case Node::RIGHT: {
      forced = (!map_.Occupied(np->x + 1, np->y + 1) && 
        map_.IsValid(np->x, np->y + 1) && 
        map_.Occupied(np->x, np->y + 1)) ||
        (!map_.Occupied(np->x + 1, np->y - 1) && 
        map_.IsValid(np->x, np->y - 1) && 
        map_.Occupied(np->x, np->y - 1));
        break;
    }
    case Node::RIGHT_UP: {
      forced = (!map_.Occupied(np->x - 1, np->y + 1) && 
        map_.IsValid(np->x - 1, np->y) && 
        map_.Occupied(np->x - 1, np->y)) ||
        (!map_.Occupied(np->x + 1, np->y - 1) &&
        map_.IsValid(np->x, np->y - 1) && 
        map_.Occupied(np->x, np->y - 1));
        break;
    }
    case Node::UP: {
      forced = (!map_.Occupied(np->x - 1, np->y + 1) &&
        map_.IsValid(np->x - 1, np->y) && 
        map_.Occupied(np->x - 1, np->y)) ||
        (!map_.Occupied(np->x + 1, np->y + 1) &&
        map_.IsValid(np->x + 1, np->y) && 
        map_.Occupied(np->x + 1, np->y));
        break;
    }
    case Node::LEFT_UP: {
      forced = (!map_.Occupied(np->x - 1, np->y - 1) &&
        map_.IsValid(np->x, np->y - 1) && 
        map_.Occupied(np->x, np->y - 1)) ||
        (!map_.Occupied(np->x + 1, np->y + 1) &&
        map_.IsValid(np->x + 1, np->y) && 
        map_.Occupied(np->x + 1, np->y));
        break;
    }
    case Node::LEFT: {
      forced = (!map_.Occupied(np->x - 1, np->y + 1) &&
        map_.IsValid(np->x, np->y + 1) && 
        map_.Occupied(np->x, np->y + 1)) ||
        (!map_.Occupied(np->x - 1, np->y - 1) &&
        map_.IsValid(np->x, np->y - 1) && 
        map_.Occupied(np->x, np->y - 1));
        break;
    }
    case Node::LEFT_DOWN: {
      forced = (!map_.Occupied(np->x - 1, np->y + 1) &&
        map_.IsValid(np->x, np->y + 1) && 
        map_.Occupied(np->x, np->y + 1)) ||
        (!map_.Occupied(np->x + 1, np->y - 1) &&
        map_.IsValid(np->x + 1, np->y) && 
        map_.Occupied(np->x + 1, np->y));
        break;
    }
    case Node::DOWN: {
      forced = (!map_.Occupied(np->x - 1, np->y - 1) &&
        map_.IsValid(np->x - 1, np->y) && 
        map_.Occupied(np->x - 1, np->y)) ||
        (!map_.Occupied(np->x + 1, np->y - 1) &&
        map_.IsValid(np->x + 1, np->y) && 
        map_.Occupied(np->x + 1, np->y));
        break;
    }
    case Node::RIGHT_DOWN: {
      forced = (!map_.Occupied(np->x + 1, np->y + 1) &&
        map_.IsValid(np->x, np->y + 1) && 
        map_.Occupied(np->x, np->y + 1)) ||
        (!map_.Occupied(np->x - 1, np->y - 1) &&
        map_.IsValid(np->x - 1, np->y) && 
        map_.Occupied(np->x - 1, np->y));
        break;
    }
    case Node::NONE:
    default: {
      printf("[GraphSearch.cpp] error dir %d\n", np->dir);
      break;
    }
  }

  return forced;
}

// Node::Ptr GraphSearch::Jump(const Node& node) const {
Node::Ptr GraphSearch::Jump(const Node& node) const {
  Node next_node = Step(node);
  next_node.f = next_node.g + map_.Heuristic(
    next_node.x, next_node.y, goal_->x, goal_->y);

  if(map_.Occupied(next_node.x, next_node.y)) {
    return nullptr;
  }

  Node::Ptr np = std::make_shared<Node>(next_node);
  if(IsSameNode(np, goal_)) {
    return np;
  }

  if(Forced(np)) {
    return np;
  }

  if(np->dir == Node::RIGHT_UP) {
    next_node.dir = Node::RIGHT;
    if(Jump(next_node) != nullptr) {
      return np;
    }
    next_node.dir = Node::UP;
    if(Jump(next_node) != nullptr) {
      return np;
    }
  } else if(np->dir == Node::LEFT_UP) {
    next_node.dir = Node::LEFT;
    if(Jump(next_node) != nullptr) {
      return np;
    }
    next_node.dir = Node::UP;
    if(Jump(next_node) != nullptr) {
      return np;
    }
  } else if(np->dir == Node::LEFT_DOWN) {
    next_node.dir = Node::LEFT;
    if(Jump(next_node) != nullptr) {
      return np;
    }
    next_node.dir = Node::DOWN;
    if(Jump(next_node) != nullptr) {
      return np;
    }
  } else if(np->dir == Node::RIGHT_DOWN) {
    next_node.dir = Node::RIGHT;
    if(Jump(next_node) != nullptr) {
      return np;
    }
    next_node.dir = Node::DOWN;
    if(Jump(next_node) != nullptr) {
      return np;
    }
  }

  return Jump(*np);
}

//对于新加入closed_list_的节点寻找可能的后继节点
void GraphSearch::ProcessNeighbours(const Node::Ptr& np) {
  if(search_type_ == Type::DIJKSTRA) {
    ProcessNeighboursDijkstra(np);
  } else if(search_type_ == Type::ASTAR) {
    ProcessNeighboursAstar(np);
  } else if(search_type_ == Type::JPS) {
    ProcessNeighboursJps(np);
  } else {
    printf("[GraphSearch.cpp] Search Type ERROR!\n");
  }
}

void GraphSearch::ProcessNeighboursDijkstra(const Node::Ptr& np) {
  auto neighbours = map_.GetNeighbours(np->x, np->y); //找到free的邻居
  for(const auto& neighbour : neighbours) {
    Node::Ptr node = std::make_shared<Node>(neighbour.x, neighbour.y);
    if(expanded_.at(node->x + node->y * map_.Width()).status != GridXY::CLOSED) { //若该邻居未closed
      if(expanded_.at(node->x + node->y * map_.Width()).status == GridXY::OPEN) { //若该邻居为expanded(在openlist)
        auto g = np->g + neighbour.cost;
        if(g < expanded_.at(node->x + node->y * map_.Width()).np->g) { //若该邻居路线更优
          auto& open_np = expanded_.at(node->x + node->y * map_.Width()).np;
          //先删除
          open_list_f_.erase(open_list_f_.find(open_np));
          //再插入
          open_np->parent = np; //parent
          open_np->g = g; //g
          open_np->f = open_np->g; //f
          open_list_f_.insert(open_np);
        }
      } else { //若该邻居为new expanded
        node->parent = np; //parent
        node->g = np->g + neighbour.cost; //g
        node->f = node->g; //f
        open_list_f_.insert(node);
        expanded_.at(node->x + node->y * map_.Width()).status = GridXY::OPEN;
        expanded_.at(node->x + node->y * map_.Width()).np = node;
      }
    }
    //!!no else, so heuristic should be consistent and admissible
  }
}

void GraphSearch::ProcessNeighboursAstar(const Node::Ptr& np) {
  auto neighbours = map_.GetNeighbours(np->x, np->y); //找到free的邻居
  for(const auto& neighbour : neighbours) {
    Node::Ptr node = std::make_shared<Node>(neighbour.x, neighbour.y);
    if(expanded_.at(node->x + node->y * map_.Width()).status != GridXY::CLOSED) { //若该邻居未closed
      if(expanded_.at(node->x + node->y * map_.Width()).status == GridXY::OPEN) { //若该邻居为expanded(在openlist)
        auto g = np->g + neighbour.cost;
        if(g < expanded_.at(node->x + node->y * map_.Width()).np->g) { //若该邻居路线更优
          auto& open_np = expanded_.at(node->x + node->y * map_.Width()).np;
          //先删除
          open_list_f_.erase(open_list_f_.find(open_np));
          //再插入
          open_np->parent = np; //parent
          open_np->g = g; //g
          open_np->f = open_np->g + map_.Heuristic(neighbour.x, neighbour.y, goal_->x, goal_->y); //f
          open_list_f_.insert(open_np);
        }
      } else { //若该邻居为new expanded
        node->parent = np; //parent
        node->g = np->g + neighbour.cost; //g
        node->f = node->g + map_.Heuristic(neighbour.x, neighbour.y, goal_->x, goal_->y); //f
        open_list_f_.insert(node);
        expanded_.at(node->x + node->y * map_.Width()).status = GridXY::OPEN;
        expanded_.at(node->x + node->y * map_.Width()).np = node;
      }
    }
    //!!no else, so heuristic should be consistent and admissible
  }
}

void GraphSearch::ProcessNeighboursJps(const Node::Ptr& np) {
  auto neighbours = PruneNeighbours(np);
  for(const auto& neighbour : neighbours) {
    auto jump_np = Jump(neighbour);
    if(jump_np != nullptr) {
      if(expanded_.at(jump_np->x + jump_np->y * map_.Width()).status != GridXY::CLOSED) { //若该邻居未closed
        if(expanded_.at(jump_np->x + jump_np->y * map_.Width()).status == GridXY::OPEN) { //若该邻居为expanded(在openlist)
          if(jump_np->g < expanded_.at(jump_np->x + jump_np->y * map_.Width()).np->g) { //若该邻居路线更优
            auto& open_np = expanded_.at(jump_np->x + jump_np->y * map_.Width()).np; //x, y
            //先删除
            open_list_f_.erase(open_list_f_.find(open_np));
            //再插入
            jump_np->parent = np;
            open_list_f_.insert(jump_np);
            open_np = jump_np;
          }
        } else { //若该邻居为new expanded
          jump_np->parent = np;
          open_list_f_.insert(jump_np);
          expanded_.at(jump_np->x + jump_np->y * map_.Width()).status = GridXY::OPEN;
          expanded_.at(jump_np->x + jump_np->y * map_.Width()).np = jump_np;
        }
      }
    }
  }
}

//搜索start_到goal_的最短路径
bool GraphSearch::Plan() {
  InitSearch();
  while(!open_list_f_.empty()) {
    Node::Ptr np(nullptr);
    PopOpen(np);
    if(IsSameNode(np, goal_))
      return true;
    ProcessNeighbours(np);
  }
  return false;
}

//搜索start_到goal_的最短路径
bool GraphSearch::Plan(
    const Node& start, 
    const Node& goal, 
    const float obs_thresh, 
    const float free_thresh,
    const uint8_t type) {
  // double start_t = 0.0;
  search_type_ = type;
  SetStart(start);
  SetGoal(goal);
  map_.SetObstacleFreeThresh(obs_thresh, free_thresh);
  bool result = false;
  if(IsSameNode(start_, goal_)) {
    path_.clear();
    path_.push_back(*start_);
    path_.push_back(*goal_);
    result = true;
  } else {
    result = Plan();
    if(result) {
      GetPath();
    }
  }
  // double path_length = 0.0;
  // for (size_t idx = 0; idx + 1 < path_.size(); ++idx) {
  //   path_length += sqrt(pow((int32_t)path_.at(idx + 1).y - (int32_t)path_.at(idx).y, 2) + 
  //     pow((int32_t)path_.at(idx + 1).x - (int32_t)path_.at(idx).x, 2));
  // }

  // double time_consume = 0.0;
  // printf(
  //   "[GraphSearch.cpp] path plan result is %d, consume is %.4f s, length is %f, area are closed(%ld), open(%ld).\n",
  //   result ? 1 : 0, time_consume, path_length, closed_list_size_, open_list_f_.size());

  return result;
}

//Plan成功后，根据closed_list回溯得到路径
const Nodes& GraphSearch::GetPath() {
  if(path_.empty() && expanded_.at(goal_->x + goal_->y * map_.Width()).status != GridXY::UNEXPANDED) {
    auto next_node = expanded_.at(goal_->x + goal_->y * map_.Width()).np;
    path_.push_front(*next_node);
    if(IsSameNode(next_node, start_))
        return path_;
    while(next_node->parent != nullptr) {
      next_node = next_node->parent;
      path_.push_front(*next_node);
      if(IsSameNode(next_node, start_))
        break;
    }
  }
  return path_;
}

Nodes GraphSearch::GetOptimizedPath() {
  auto path = Critical2Path(path_, 1.0);
  if (!IsSameNode(start_, goal_)) {
    path = OptimizePath(map_, path, map_.ThreshObstacle());
    std::reverse(path.begin(), path.end());
    path = Critical2Path(path, 1.0);
    path = OptimizePath(map_, path, map_.ThreshObstacle());
    std::reverse(path.begin(), path.end());
  }
  return path;
}

Nodes common::planning::OptimizePath(
    const DistanceMap& map, 
    const Nodes& path, 
    const float thresh_distance) {
  if(path.size() > 2) {
    Nodes critical_path;
    critical_path.push_back(path.front());
    size_t i_start = 0;
    size_t i_checked = 1;
    float dir_start = CalLineDir(path.at(i_start).x, path.at(i_start).y, 
      path.at(i_checked).x, path.at(i_checked).y);
    for(size_t i_to_check = 2; i_to_check < path.size(); ++ i_to_check) {
      float dir = CalLineDir(path.at(i_start).x, path.at(i_start).y, 
        path.at(i_to_check).x, path.at(i_to_check).y);
      if(fabs(dir_start - dir) < 1e-3) {
        i_checked = i_to_check;
        continue;
      } else {
        if(IsPathFree(map, path.at(i_start), path.at(i_to_check), thresh_distance)) {
          i_checked = i_to_check;
          continue;
        } else {
          critical_path.push_back(path.at(i_checked));
          i_start = i_checked;
          i_checked = i_to_check;
          dir_start = CalLineDir(path.at(i_start).x, path.at(i_start).y, 
            path.at(i_checked).x, path.at(i_checked).y);
        }
      }
    }
    critical_path.push_back(path.back());
    return critical_path;
  }
  return path;
}

Nodes common::planning::Critical2Path(
    const Nodes& critical_path, 
    const float resolution) {
  Nodes path{};
  for (size_t i = 0; i + 1 < critical_path.size(); ++i) {
    float x_start = Int2Float(critical_path.at(i).x);
    float y_start = Int2Float(critical_path.at(i).y);
    float x_end = Int2Float(critical_path.at(i + 1).x);
    float y_end = Int2Float(critical_path.at(i + 1).y);
    float dx = x_end - x_start;
    float dy = y_end - y_start;
    float len = sqrt(dx * dx + dy * dy);
    float del_cos = resolution * dx / len;
    float del_sin = resolution * dy / len;
    int64_t n = static_cast<int64_t>(floor(len / resolution));
    for (int64_t j = 0; j <= n; ++j) {
      Node n(Float2Int(x_start + j * del_cos), Float2Int(y_start + j * del_sin));
      if (path.empty() || path.back().x != n.x || path.back().y != n.y) {
        if (path.size() >= 2) {
          Node pre_pre_node(path.at(path.size() - 2));
          float len = sqrt((pre_pre_node.x - n.x) * (pre_pre_node.x - n.x) +
                           (pre_pre_node.y - n.y) * (pre_pre_node.y - n.y));
          if (len < 2.0) {
            path.pop_back();
          }
        }
        path.push_back(n);
      }
    }
  }
  if (!critical_path.empty() &&
      (path.empty() || 
       path.back().x != critical_path.back().x ||
       path.back().y != critical_path.back().y)) {
    path.push_back(critical_path.back());
  }
  return path;
}

bool common::planning::IsPathFree(
    const DistanceMap& map, 
    const Node& start, 
    const Node& end, 
    const float thresh_distance) {
  if(start.x == end.x && start.y == end.y) {
    return map.GetGrid(start.x, start.y) < thresh_distance;
  }

  //bresenham --> get pixels on the line((x0, y0) --> (x1, y1))
  int64_t x0 = start.x;
  int64_t y0 = start.y;
  int64_t x1 = end.x;
  int64_t y1 = end.y;
  bool steep = abs(y1 - y0) > abs(x1 - x0);
  if(steep) {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }
  if(x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }
  int64_t deltax = x1 - x0;
  int64_t deltay = abs(y1 - y0);
  int64_t error = deltax / 2;
  int64_t y = y0;
  int64_t ystep = y0 < y1 ? 1 : -1;
  for(int64_t x = x0; x <= x1; ++x) {
    //steep is false --> pixel is (x, y); steep is true --> pixel is (y, x)
    if(map.GetGrid(steep ? y : x, steep ? x : y) < thresh_distance) {
      return false;
    }
    error = error - deltay;
    if(error < 0) {
        y = y + ystep;
        error = error + deltax;
    }
  }
  return true;
}

Nodes common::planning::Bresenham(const Node& start, const Node& end) {
  //Bresenham --> get pixels on the line((x0, y0) --> (x1, y1))
  Nodes points{};
  int64_t x0 = start.x;
  int64_t y0 = start.y;
  int64_t x1 = end.x;
  int64_t y1 = end.y;
  bool steep = abs(y1 - y0) > abs(x1 - x0);
  if(steep) {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }
  if(x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }
  int64_t deltax = x1 - x0;
  int64_t deltay = abs(y1 - y0);
  int64_t error = deltax / 2;
  int64_t y = y0;
  int64_t ystep = y0 < y1 ? 1 : -1;
  for(int64_t x = x0; x <= x1; ++x) {
    //steep is false --> pixel is (x, y); steep is true --> pixel is (y, x)
    points.emplace_back(steep ? y : x, steep ? x : y);
    error = error - deltay;
    if(error < 0) {
        y = y + ystep;
        error = error + deltax;
    }
  }
  return points;
}

void GraphSearch::PrintPlanResult() const {
  std::ostringstream map_to_print;
  map_to_print << "\n-------------- map and path --------------\n";
  XYNodePtrs path;
  for(const auto& p : path_) {
    Node::Ptr node = std::make_shared<Node>(p.x, p.y);
    path.insert(node);
  }

  for(int64_t y = map_.Height(); y > 0; -- y) {
    for(int64_t x = 0; x < map_.Width(); ++ x) {
      Node::Ptr node = std::make_shared<Node>(x, y-1);
      if(start_->x == x && start_->y == y-1)
        map_to_print << 'S';
      else if(goal_->x == x && goal_->y == y-1)
        map_to_print << 'G';
      else if(map_.Occupied(x, y-1))
        map_to_print << '*';
      else if(path.find(node) != path.end())
        map_to_print << '1';
      else
        map_to_print << ' ';
    }
    map_to_print << '\n';
  }
  printf("[GraphSearch.cpp] %s\n", map_to_print.str().c_str());
}

void GraphSearch::PubPlanResult() {
  // TODO: send ros topic to be visualized by rviz 
}